package pathfinding;

import java.util.ArrayList;
import java.util.Collection;

import robotutils.planning.AStarStats;

public abstract class AStarGrid extends AStarStats<GridCell>{
	
	protected static final double diagCost = Math.sqrt(2);
	
	protected abstract boolean isReserved(GridCell c, long timeStart, long timeEnd);
	
	private static final long serialVersionUID = 1L;
	
	public static final double reservedCost = Double.POSITIVE_INFINITY;

	protected Collection<GridCell> nbrs(GridCell c) {
		Collection<GridCell> result = new ArrayList<GridCell> (8);
		
		result.add(c.east()     );
		result.add(c.west()     );
		result.add(c.north());
		result.add(c.south()    );
		result.add(c.northeast());
		result.add(c.southeast());
		result.add(c.southwest());
		result.add(c.northwest());
		
		return result;
	}

	@Override
	protected Collection<GridCell> succ(GridCell s) {
		return nbrs(s);
	}


	@Override
	protected double h(GridCell a, GridCell b) {
		//return GridGeometry.manhattanDist(a, b);
		return GridGeometry.amountOfSquaresBetween(a,b);
	}


	@Override
	protected double c(GridCell a, GridCell b) {
		if (isReserved(b, 0, Long.MAX_VALUE)) {
			return reservedCost;
		}
		if (isReserved(a, 0, Long.MAX_VALUE)) {
			return reservedCost;
		}
		
		return baseCost(a,b);
	}
	
	protected double baseCost(GridCell a, GridCell b) {
		if (a.equals(b)) {
			return 0;
		}
		int mdist = GridGeometry.manhattanDist(a, b); 
		if (mdist == 1)
		{
			return 1;
		}
		else if (mdist == 2)
		{
			return diagCost;
		}
		else {
			throw new IllegalStateException("From " + a + " to " + b);
		}
	}

}
